import DobotDllType as dType


if __name__ == "__main__":
    # Load Dll and get the CDLL object
    api = dType.load()
    
    # Connect Dobot
    state = dType.ConnectDobot(api, "COM9", 115200)[0]
    if state != dType.DobotConnect.DobotConnect_NoError:
        print("Failed to connect to Dobot")
        exit(1)
    
    # Clear command queue
    dType.SetQueuedCmdClear(api)
    
    # Get current pose
    pose = dType.GetPose(api)
    print(f"Current Pose: {pose[0:4]}")
    
    # Disconnect Dobot
    dType.DisconnectDobot(api)